package Experiment3;

import edu.princeton.cs.algs4.*;

import java.util.ArrayList;
import java.util.List;

/** 
 * <p>基于{@code edu.priceton.cs.algs4.DijkstraUndirectedSP}的Dijkstra实现，针对多次查询以及大数据量优化的Dijkstra算法
 * <p>
 * 具体优化方向有：
 *      1. 算法计算出指定目的节点时就停止计算，并保留计算结果
 *      2. 算法初始化distTo时只对上次修改过的节点的distTo进行初始化，这需要维护一个包
 *      含上次修改过的节点的容器
 *      3. 当本次查询与上次查询的起点相同时，将基于上次查询的计算结果进行计算
 *      4. 使用基于多路堆实现的索引优先队列
 *      5. 使用类似A*算法的思想优化
 * 
 * @author Chen Junjie
 */
public class MyDijkstra {
    private double[] distTo; // distTo[v] = distance of shortest s->v path
    private Edge[] edgeTo; // edgeTo[v] = last edge on shortest s->v path
    private double[] total_cost_to; //储存原点到节点的总代价，为实际代价+估算代价
    private IndexMultiwayMinPQ<Double> pq; // priority queue of vertices

    private List<Integer> moddified; //记录上次修改过的节点
    private EuclidienGraph graph;
    private int last_source; //记录上次查询的起始节点
    private int current_destination;
    private boolean[] marked;

    public MyDijkstra(EuclidienGraph G) {
        for (Edge e : G.edges()) {
            if (e.weight() < 0)
                throw new IllegalArgumentException("edge " + e + " has negative weight");
        }

        distTo = new double[G.V()];
        edgeTo = new Edge[G.V()];
        total_cost_to=new double[G.V()];
        marked=new boolean[G.V()];

        this.graph=G;
        last_source=-1; //对象实例化时不存在上次查询的起始节点，故初始化为-1
        moddified=new ArrayList<Integer>();
        
        //初始化所有节点的distTo和总代价
        for (int v=0;v<graph.V();v++) {
            distTo[v]=Double.POSITIVE_INFINITY;
            total_cost_to[v]=Double.POSITIVE_INFINITY;
        }
    }

    //指定起始节点与目的节点计算最短路径
    //通过distance与path方法获取最短距离与最短路径
    public void calculateShortPath(int source,int destination){
        validateVertex(source);
        validateVertex(destination);
        this.current_destination=destination;

        if(source!=last_source){
            for (int v : moddified) {
                distTo[v]=Double.POSITIVE_INFINITY;
                total_cost_to[v]=Double.POSITIVE_INFINITY;
            }
            moddified.clear();

            edgeTo=new Edge[graph.V()];

            distTo[source] = 0.0;
            total_cost_to[source]=0.0;
            marked=new boolean[graph.V()];
            moddified.add(source);
            pq = new IndexMultiwayMinPQ<Double>(graph.V(),3);
            pq.insert(source, total_cost_to[source]);
        }

        //如果本次查询的路径在上次的查询中已经计算出来了，则直接利用上次查询的结果。
        if(source==last_source&&!pq.contains(destination)&&hasPathTo(destination)) return;

        last_source=source;
        // relax vertices in order of distance from s
        
        while (!pq.isEmpty()) {
            int v = pq.delMin();
            marked[v]=true;
            for (Edge e : graph.adj(v))
                /**
                 * 此处相比algs4原本的算法增加了该边是否属于横切边的判断
                 * 如果没有该判断可能会出现两个节点的edgeTo值都为连接两个节点的边，导致路径错误以及打印异常
                 * 出现该现象的原因尚未分析
                 */
                if(!marked[e.other(v)]) 
                    relax(e, v);
            if(v==destination) break; //检查目的节点的最短路径是否已经求出
        }

        // check optimality conditions
        assert check(graph, source);
    }

    // relax edge e and update pq if changed
    private void relax(Edge e, int v) {
        int w = e.other(v);

        //计算新总代价
        
        double new_cost= total_cost_to[v] + e.weight()
                   +graph.getPoint(w).distanceTo(graph.getPoint(current_destination))
                   -graph.getPoint(v).distanceTo(graph.getPoint(current_destination));
        

        //double new_cost=distTo[v]+e.weight();
        /*
         * 通过同时记录总代价与实际代价，可以实现朴素实现A*算法
         */
        if (total_cost_to[w] > new_cost) {
            total_cost_to[w] = new_cost;
            distTo[w]=distTo[v]+e.weight();
            edgeTo[w] = e;
            if (pq.contains(w))
                pq.decreaseKey(w, total_cost_to[w]);
            else{
                pq.insert(w, total_cost_to[w]);
                moddified.add(w);   //第一次放松v-w的边，导致distTo[w]的值修改，保存修改记录
            }
                
        }
    }

    public double distance(){
        return distTo(current_destination);
    }

    public Iterable<Edge> path(){
        return pathTo(current_destination);
    }

    public boolean hasPath(){
        return hasPathTo(current_destination);
    }

    public double distTo(int v) {
        validateVertex(v);
        return distTo[v];
    }

    public boolean hasPathTo(int v) {
        validateVertex(v);
        return distTo[v] < Double.POSITIVE_INFINITY;
    }

    public Iterable<Edge> pathTo(int v) {
        validateVertex(v);
        if (!hasPathTo(v))
            return null;
        Stack<Edge> path = new Stack<Edge>();
        int x = v;
        for (Edge e = edgeTo[v]; e != null; e = edgeTo[x]) {
            path.push(e);
            x = e.other(x);
        }
        return path;
    }

    // check optimality conditions:
    // (i) for all edges e = v-w: distTo[w] <= distTo[v] + e.weight()
    // (ii) for all edge e = v-w on the SPT: distTo[w] == distTo[v] + e.weight()
    private boolean check(EuclidienGraph G, int s) {

        // check that edge weights are non-negative
        for (Edge e : G.edges()) {
            if (e.weight() < 0) {
                System.err.println("negative edge weight detected");
                return false;
            }
        }

        // check that distTo[v] and edgeTo[v] are consistent
        if (distTo[s] != 0.0 || edgeTo[s] != null) {
            System.err.println("distTo[s] and edgeTo[s] inconsistent");
            return false;
        }
        for (int v = 0; v < G.V(); v++) {
            if (v == s)
                continue;
            if (edgeTo[v] == null && distTo[v] != Double.POSITIVE_INFINITY) {
                System.err.println("distTo[] and edgeTo[] inconsistent");
                return false;
            }
        }

        // check that all edges e = v-w satisfy distTo[w] <= distTo[v] + e.weight()
        for (int v = 0; v < G.V(); v++) {
            for (Edge e : G.adj(v)) {
                int w = e.other(v);
                if (distTo[v] + e.weight() < distTo[w]) {
                    System.err.println("edge " + e + " not relaxed");
                    return false;
                }
            }
        }

        // check that all edges e = v-w on SPT satisfy distTo[w] == distTo[v] +
        // e.weight()
        for (int w = 0; w < G.V(); w++) {
            if (edgeTo[w] == null)
                continue;
            Edge e = edgeTo[w];
            if (w != e.either() && w != e.other(e.either()))
                return false;
            int v = e.other(w);
            if (distTo[v] + e.weight() != distTo[w]) {
                System.err.println("edge " + e + " on shortest path not tight");
                return false;
            }
        }
        return true;
    }

    // throw an IllegalArgumentException unless {@code 0 <= v < V}
    private void validateVertex(int v) {
        int V = distTo.length;
        if (v < 0 || v >= V)
            throw new IllegalArgumentException("vertex " + v + " is not between 0 and " + (V - 1));
    }

    public static void main(String[] args) {
        MyDijkstra m=new MyDijkstra(new InputData("usa.txt").getGraph());
        m.calculateShortPath(1222, 5898);
        System.out.println(m.distance()+" "+m.path());  
    }

}

